#include <opencv2/opencv.hpp>
#include <iostream>
#include <chrono>
#include <sys/time.h>
#include <unistd.h>

using namespace std;
using namespace cv;

#define MILLION 1000000L

int main(int, char *argv[])
{
    Mat in_image, out_image;
    //读取原始图像
    in_image = imread("../test.png", IMREAD_GRAYSCALE);
    if (in_image.empty()) {
        //检查是否读取图像
        cout << "Error! Input image cannot be read...\n";
        return -1;
    }else{
        cout << "Read image OK :) " << endl;
        cout << "And its properties are list as follows:" << endl;
        cout << "> rows: " << in_image.rows << endl;
        cout << "> cols: " << in_image.cols << endl;
        cout << "> channels/depthts: " << in_image.channels() << endl;
        cout << "> elemSize: " << in_image.elemSize() << endl;
        cout << "> step: " << in_image.step1() << endl;
    }
    // 使用 getOptimalNewCameraMatrix + initUndistortRectifyMap + remap 矫正图像
    // 当alpha=1时，所有像素均保留，但存在黑色边框，矫正后的图像如图1所示。
    // 当alpha=0时，损失最多的像素，没有黑色边框，矫正后的图像如图2所示。
    // 畸变参数
    double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05;
    // 内参
    double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;
    // 去畸变以后的图
    int rows = in_image.rows;
    int cols = in_image.cols;
    cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1);   
    // 计算去畸变后图像的内容
    const cv::Mat K = ( cv::Mat_<double> ( 3,3 ) << 
        fx,  0.0,  cx, 
        0.0, fy,   cy, 
        0.0, 0.0,  1.0 );
    const cv::Mat D = ( cv::Mat_<double> ( 4,1 ) << k1, k2, p1, p2 );
    cv::Mat map1, map2;
    cv::Size imageSize(cols, rows);
    const double alpha = 0;
    cv::Mat NewCameraMatrix = getOptimalNewCameraMatrix(K, D, imageSize, alpha, imageSize, 0);
    initUndistortRectifyMap(K, D, cv::Mat(), NewCameraMatrix, imageSize, CV_16SC2, map1, map2);

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    /**************************************************************/
    remap(in_image, image_undistort, map1, map2, cv::INTER_LINEAR); // INTER_LINEAR
    /**************************************************************/
	chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
	chrono::duration<double> time_used_A = chrono::duration_cast<chrono::duration<double>>(t2 - t1)*MILLION;
    cout << "Method cost " << time_used_A.count() << "us" << endl;

    // 画图去畸变前后图像
    cv::imshow("image raw", in_image);
    cv::imshow("image undistorted", image_undistort);
    cv::waitKey();

    return 0;
}
